﻿#include <QJsonObject>
#include <QJsonArray>

#include <osg/Geometry>

#include <gp_Circ2d.hxx>
#include <GCE2d_MakeArcOfCircle.hxx>
#include <GCE2d_MakeSegment.hxx>
#include <GCE2d_MakeCircle.hxx>
#include <Standard_Type.hxx>
#include <Geom2d_Circle.hxx>
#include <Geom2dAPI_InterCurveCurve.hxx>
#include <Geom2dAPI_ProjectPointOnCurve.hxx>

#include "geometricutility.h"
#include "DblPoint.h"
#include "Arc2.h"
#include "Arc2D.h"
#include "LineStrip2D.h"
#include "JsonReader.h"
#include "GLSLPrograms.h"
#include "Pline2D.h"
#include "GetTriangles.h"
#include "Plane.h"
#include "OcctOsg.h"
#include "OsgUtil.h"
#include "OcctShapeProp.h"

CDblPoint osgPointToCDblPoint(const osg::Vec3d& point)
{
	return CDblPoint(point.x(), point.y());
}
 
osg::Vec3d cDblPointToOsgPoint(const CDblPoint& point)
{
	return osg::Vec3d(point.x,point.y,0.0);
}

bool calBugleFrom3P(CDblPoint startPt, CDblPoint secondPt, CDblPoint endPt,double& bugle)
{
	
	CDblPoint center;
	double radius;
	double startAng;
	double sweepAng;
	bool success = CalArcFrom3P_SSD(startPt, secondPt, endPt, center, radius, startAng, sweepAng);
	if (success)
	{
		CalSEHFromArc(center, radius, startAng, sweepAng, startPt,endPt, bugle);
		return true;
	}
	else
	{
		return false;
	}
}

struct PlinePrivate
{
	PlinePrivate()
		: m_rep3D(new osg::Geometry())
	{
		m_rep3D->setNodeMask(D2CurveMask);
		osg::ref_ptr<osg::Program> program = createEdgeProgram();
		auto ss = m_rep3D->getOrCreateStateSet();
		ss->setAttributeAndModes(program, osg::StateAttribute::ON);
	}
	osg::ref_ptr<osg::Geometry> m_rep3D;
	osg::Vec4 m_color = { 1, 1, 1, 1 };
	bool m_selected = false;
	std::vector<PlineNode> m_nodes;
	mutable std::vector<osg::Vec3d> m_handles;
};

Pline2D::Pline2D(INode* parent)
	: Ent2DCurve(parent)
	, d_ptr(new PlinePrivate)
{
}

void Pline2D::addNode(const osg::Vec3d & point, double bulge)
{
	d_ptr->m_nodes.push_back({point,bulge});
	addFlags(UPDATE_GEOMETRY);
}

const std::vector<PlineNode>& Pline2D::node() const
{
	return d_ptr->m_nodes;
}

void Pline2D::setNode(const std::vector<PlineNode>& nodes)
{
	d_ptr->m_nodes = nodes;
	addFlags(UPDATE_GEOMETRY);
}

std::shared_ptr<Ent> Pline2D::clone() const
{
	auto pline2D = std::make_shared<Pline2D>(getParent());
	pline2D->copy(*this);
	return pline2D;
}

void Pline2D::setColor(const osg::Vec4 &color)
{
	if (d_ptr->m_color != color)
	{
		d_ptr->m_color = color;
		addFlags(UPDATE_GEOMETRY);
	}
}

int Pline2D::getNodeCount() const
{
	return d_ptr->m_nodes.size();
}

const osg::Vec4 & Pline2D::color() const
{
	return d_ptr->m_color;
}


void Pline2D::update3D()
{
	if (flags())
	{
		const osg::Vec4f& selectedColor = osg::Vec4f(0.f, 0.8f, 0.f, 1.f);
		auto va = new osg::Vec3Array();
		int nodeCount = getNodeCount();
		for (int i = 0; i < nodeCount - 1; i++)
		{
			if (d_ptr->m_nodes[i].m_bulge == 0)
			{
				va->push_back(d_ptr->m_nodes[i].m_point);
				if (i == nodeCount-2)
				{
					va->push_back(d_ptr->m_nodes[i + 1].m_point);
				}
			}
			else
			{
				CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point),osgPointToCDblPoint(d_ptr->m_nodes[i+1].m_point),d_ptr->m_nodes[i].m_bulge);
				auto points = getArcPoints(arc.StartAngle(), arc.EndAngle(), arc.radius, cDblPointToOsgPoint(arc.center), osg::Vec3d(0, 0, 1), 3);
				if (points.size() != 0)
				{
					double length = (points[0].x() - d_ptr->m_nodes[i].m_point.x())*(points[0].x() - d_ptr->m_nodes[i].m_point.x()) +
						(points[0].y() - d_ptr->m_nodes[i].m_point.y())*(points[0].y() - d_ptr->m_nodes[i].m_point.y());
					if (length > 0.0001)
					{
						std::reverse(points.begin(), points.end());
					}
				}	
				appendPoints(va, points);
			}
		}
		if (nodeCount > 1)
		{
			calHandles();
		}
		d_ptr->m_rep3D->setVertexArray(va);
		osg::ref_ptr<osg::Vec4Array> ca = new osg::Vec4Array();

		if (d_ptr->m_selected)
			ca->push_back(selectedColor);
		else
			ca->push_back(d_ptr->m_color);

		d_ptr->m_rep3D->setColorArray(ca.get(), osg::Array::BIND_OVERALL);

		d_ptr->m_rep3D->getPrimitiveSetList().clear();
		d_ptr->m_rep3D->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, va->size()));

		d_ptr->m_rep3D->dirtyGLObjects();
		removeFlags(0xFFFFFFFF);
	}
}

osg::Node * Pline2D::getRep3D() const
{
	return d_ptr->m_rep3D.get();
}

void Pline2D::setSelected(bool val)
{
	if (d_ptr->m_selected != val)
	{
		d_ptr->m_selected = val;
		addFlags(UPDATE_GEOMETRY);
	}
}

void Pline2D::save(QJsonObject & json) const
{
	Ent2DCurve::save(json);
	json["color"] = saveVec4f(d_ptr->m_color);
	QJsonArray array;
	for (auto node : d_ptr->m_nodes)
	{
		QJsonObject jsonObject;
		jsonObject.insert("point", saveVec3f(node.m_point));
		jsonObject.insert("bulge", node.m_bulge);
		array.append(jsonObject);
	}
	json["nodes"] = array;
}

void Pline2D::load(const QJsonObject& json, JsonReader& reader)
{
	Ent2DCurve::load(json, reader);
	d_ptr->m_color = loadVec4f(json["color"].toArray());
	for (const auto& object : json["nodes"].toArray())
	{
		auto nodeJson = object.toObject();
		PlineNode node(loadVec3f(nodeJson["point"].toArray()),nodeJson["bulge"].toDouble());
		d_ptr->m_nodes.push_back(node);
	}
}



void Pline2D::calHandles() const
{
	d_ptr->m_handles.clear();
	int nodeCount = getNodeCount();
	for (int i = 0; i < nodeCount - 1; i++)
	{
		if (d_ptr->m_nodes[i].m_bulge == 0)
		{
			d_ptr->m_handles.push_back(d_ptr->m_nodes[i].m_point);
		}
		else
		{
			CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point), osgPointToCDblPoint(d_ptr->m_nodes[i + 1].m_point), d_ptr->m_nodes[i].m_bulge);
			d_ptr->m_handles.push_back(d_ptr->m_nodes[i].m_point);
			d_ptr->m_handles.push_back(cDblPointToOsgPoint(arc.MidPoint()));
		}
	}
	if (nodeCount > 1)
		d_ptr->m_handles.push_back(d_ptr->m_nodes[nodeCount - 1].m_point);
}

std::vector<osg::Vec3d> Pline2D::handles() const
{
	if (flags())
	{
		calHandles();
	}
	return d_ptr->m_handles;
}

void Pline2D::move(const osg::Vec3d& offset)
{
	int nodeCount = getNodeCount();
	for (int i = 0; i < nodeCount; i++)
	{
		d_ptr->m_nodes[i].m_point += offset;
	}
	addFlags(UPDATE_GEOMETRY);
}

void Pline2D::scale(const osg::Vec3d & center, double scale)
{
	for (auto& node : d_ptr->m_nodes)
	{
		node.m_point = center + (node.m_point - center) * scale;
	}
}

void Pline2D::mirror(const osg::Vec3d & start, const osg::Vec3d & end)
{
	double sign = start == end ? 1.0 : -1.0;
	for (auto& node:d_ptr->m_nodes)
	{
		osg::Vec3d point = mirror2D (start, end,node.m_point);
		node.m_point = point;
		node.m_bulge = sign * node.m_bulge;
	}
	addFlags(UPDATE_GEOMETRY);
}

void Pline2D::stretch(int index, const osg::Vec3d &offset)
{
	
	if (d_ptr->m_handles.empty())
		calHandles();
	int nodeCount = getNodeCount();
	int indexCount = 0;
	for (int i = 0; i < nodeCount; i++)
	{
		if (d_ptr->m_nodes[i].m_bulge == 0)
		{
			if (indexCount == index)
			{
				d_ptr->m_nodes[i].m_point += offset;
				break;
			}		
			indexCount += 1;
		}
		else
		{
			if (indexCount == index)
			{
				d_ptr->m_nodes[i].m_point += offset;
				break;
			}
			//拉伸夹点为圆弧中点
			if ((indexCount + 1)== index)
			{
			calBugleFrom3P(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point),
						   osgPointToCDblPoint(d_ptr->m_handles[index] + offset),
						   osgPointToCDblPoint(d_ptr->m_nodes[i + 1].m_point), d_ptr->m_nodes[i].m_bulge);
				break;
			}
			indexCount += 2;
		}
	}
	addFlags(UPDATE_GEOMETRY);
}

void Pline2D::copy(const Ent& ent)
{
	auto pline2d = dynamic_cast<const Pline2D*>(&ent);
	if (pline2d)
	{
		d_ptr->m_color = pline2d->color();
		d_ptr->m_nodes = pline2d->node();
		d_ptr->m_handles = pline2d->handles();
	}
}

void Pline2D::rotate(const double angle, const osg::Vec2d & basePoint)
{
	for (auto& node : d_ptr->m_nodes)
	{
		rotate2D(basePoint.x(),basePoint.y(),angle,node.m_point);
	}
	addFlags(UPDATE_GEOMETRY);
}

bool Pline2D::explode(std::vector<std::shared_ptr<Ent>>& result) const
{
	int nodeCount = getNodeCount();
	if (nodeCount > 2)
	{
		for (int i = 0; i < nodeCount - 1; i++)
		{
			if (d_ptr->m_nodes[i].m_bulge == 0)
			{
				std::shared_ptr<LineStrip2D> line = std::make_shared<LineStrip2D>(getParent());
				std::vector <osg::Vec3d> points;
				points.push_back(d_ptr->m_nodes[i].m_point);
				points.push_back(d_ptr->m_nodes[i + 1].m_point);
				line->setPoints(points);
				line->setColor(d_ptr->m_color);
				result.push_back(line);
			}
			else
			{
				std::shared_ptr<Arc2D> arc2D = std::make_shared<Arc2D>(getParent());
				CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point), osgPointToCDblPoint(d_ptr->m_nodes[i + 1].m_point), d_ptr->m_nodes[i].m_bulge);
				arc2D->set(osg::Vec3d(arc.center.x, arc.center.y, 0), arc.radius, arc.StartAngle(), arc.EndAngle());
				arc2D->setColor(d_ptr->m_color);
				result.push_back(arc2D);
			}
		}
		return true;
	}
	else
	{
		return false;
	}	
}

std::vector<Handle(Geom2d_Curve)> Pline2D::getCurve2d() const
{
	std::vector<Handle(Geom2d_Curve)> result;
	
	for (int i = 0; i < int(d_ptr->m_nodes.size() - 1); i++)
	{
		if (d_ptr->m_nodes[i].m_bulge == 0)
		{
			GCE2d_MakeSegment maker(toPnt2d(d_ptr->m_nodes[i].m_point), toPnt2d(d_ptr->m_nodes[i + 1].m_point));
			if (maker.IsDone())
			{
				result.push_back(maker.Value());
			}
		}
		else
		{
			CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point), osgPointToCDblPoint(d_ptr->m_nodes[i + 1].m_point), d_ptr->m_nodes[i].m_bulge);
			gp_Circ2d cir;
			cir.SetLocation(toPnt2d(cDblPointToOsgPoint(arc.center)));
			cir.SetRadius(arc.radius);
			GCE2d_MakeArcOfCircle maker(cir, arc.StartAngle(), arc.EndAngle());
			if (maker.IsDone())
			{
				auto trimmedcurve = maker.Value();
				double dx = d_ptr->m_nodes[i + 1].m_point.x() - trimmedcurve->EndPoint().X();
				double dy = d_ptr->m_nodes[i + 1].m_point.y() - trimmedcurve->EndPoint().Y();
				if (abs(dx) > 2E-5 || abs(dy) > 2E-5)
				{
					trimmedcurve->Reverse();
				}
				result.push_back(trimmedcurve);
			}
		}
	}	
	return result;
}

std::shared_ptr<Ent2DCurve> Pline2D::extend(bool front, double param, const gp_Pnt2d& point) const
{
	auto cloneEnt = std::static_pointer_cast<Pline2D>(clone());
	if (front)
	{
		//线段
		if (d_ptr->m_nodes[0].m_bulge == 0)
		{
			cloneEnt->d_ptr->m_nodes[0].m_point = osg::Vec3d(point.X(),point.Y(),0);
		}
		else
		{
			auto curves = getCurve2d();
			auto trimmedCurve = Handle(Geom2d_TrimmedCurve)::DownCast(curves[0].get());
			double lp = trimmedCurve->LastParameter();
			double fp = trimmedCurve->FirstParameter();
			while (param > lp)
			{
				lp += 2 * M_PI;
			}
			double startAng = param / M_PI * 180;
			double sweepAng = (lp - param) / M_PI * 180;
			if (sweepAng > 360.0)
			{
				sweepAng -= 360.0;
			}
			double crossVec = (cloneEnt->d_ptr->m_nodes[0].m_point.x() - point.X())*(cloneEnt->d_ptr->m_nodes[1].m_point.y() - point.Y()) -
							  (cloneEnt->d_ptr->m_nodes[0].m_point.y() - point.Y())*(cloneEnt->d_ptr->m_nodes[1].m_point.x() - point.X());
			if (crossVec < 0)
			{
				sweepAng = -sweepAng;
			}
			auto baseCurve = trimmedCurve->BasisCurve();
			if (baseCurve->IsKind(STANDARD_TYPE(Geom2d_Circle)))
			{
				auto circle2d = Handle(Geom2d_Circle)::DownCast(baseCurve);
				auto center = circle2d->Location();
				double radius = circle2d->Radius();
				CDblPoint start;
				CDblPoint end;
				double bugle;
				CalSEHFromArc({center.X(),center.Y()},radius,startAng,sweepAng,start,end,bugle);
				cloneEnt->d_ptr->m_nodes[0].m_bulge = bugle;
				cloneEnt->d_ptr->m_nodes[0].m_point = osg::Vec3d(point.X(), point.Y(), 0);
			}

		}
	}
	else
	{
		//线段
		int nodeCount = getNodeCount();
		if (d_ptr->m_nodes[nodeCount-2].m_bulge == 0)
		{
			cloneEnt->d_ptr->m_nodes[nodeCount - 1].m_point = osg::Vec3d(point.X(), point.Y(), 0);
		}
		else
		{
			auto curves = getCurve2d();
			int size = curves.size();
			auto trimmedCurve = Handle(Geom2d_TrimmedCurve)::DownCast(curves[size-1].get());
			double fp = trimmedCurve->FirstParameter();
			while (param < fp)
				param += 2 * M_PI;
			double startAng = fp / M_PI * 180;
			double sweepAng = (param - fp) / M_PI * 180;
			if (sweepAng > 360.0)
			{
				sweepAng -= 360.0;
			}
			int nodeCount = cloneEnt->getNodeCount();
			double crossVec = (cloneEnt->d_ptr->m_nodes[nodeCount - 1].m_point.x() - point.X())*(cloneEnt->d_ptr->m_nodes[nodeCount - 2].m_point.y() - point.Y()) -
							  (cloneEnt->d_ptr->m_nodes[nodeCount - 1].m_point.y() - point.Y())*(cloneEnt->d_ptr->m_nodes[nodeCount - 2].m_point.x() - point.X());
			if (crossVec > 0)
			{
				sweepAng = -sweepAng;
			}
			auto baseCurve = trimmedCurve->BasisCurve();
			if (baseCurve->IsKind(STANDARD_TYPE(Geom2d_Circle)))
			{
				auto circle2d = Handle(Geom2d_Circle)::DownCast(baseCurve);
				auto center = circle2d->Location();
				double radius = circle2d->Radius();
				CDblPoint start;
				CDblPoint end;
				double bugle;
				CalSEHFromArc({ center.X(),center.Y() }, radius, startAng, sweepAng, start, end, bugle);
				cloneEnt->d_ptr->m_nodes[nodeCount - 2].m_bulge = bugle;
				cloneEnt->d_ptr->m_nodes[nodeCount - 1].m_point = osg::Vec3d(point.X(), point.Y(), 0);
			}
		}
	}
	return cloneEnt;
}

std::vector<std::shared_ptr<Ent2DCurve>> Pline2D::trim(const SectionParam& from, const SectionParam& to) const
{
	std::vector<std::shared_ptr<Ent2DCurve>> result;
	auto curve2ds = getCurve2d();
	SectionParam start, end;
	getStartEndParam(curve2ds, start, end);
	if (start < from)
	{
		auto cloneEnt = std::static_pointer_cast<Pline2D>(clone());
		cloneEnt->d_ptr->m_nodes.resize(from.sectionNo + 1);
		auto fp = curve2ds[from.sectionNo]->FirstParameter();
		auto lp = curve2ds[from.sectionNo]->LastParameter();
		if (from.param > fp)
		{
			auto pt = curve2ds[from.sectionNo]->Value(from.param);
			if (d_ptr->m_nodes[from.sectionNo].m_bulge != 0)
			{
				auto curve2dFrom = curve2ds[from.sectionNo];
				gp_Pnt2d startPt;
				curve2dFrom->D0(curve2dFrom->FirstParameter(), startPt);
				gp_Pnt2d endPt;
				curve2dFrom->D0(curve2dFrom->LastParameter(), endPt);
				double startAng = fp / M_PI * 180;
				double sweepAng = (from.param - fp) / M_PI * 180;
				while (sweepAng>360)
					sweepAng -= 360;
				double crossVec = (pt.X() - startPt.X())*(endPt.Y() - startPt.Y()) - (pt.Y() - startPt.Y())*(endPt.X() - startPt.X());
				if (crossVec < 0)
				{
					sweepAng = -sweepAng;
				}
				auto trimmedCurve = Handle(Geom2d_TrimmedCurve)::DownCast(curve2ds[from.sectionNo].get());
				auto baseCurve = trimmedCurve->BasisCurve();
				if (baseCurve->IsKind(STANDARD_TYPE(Geom2d_Circle)))
				{
					auto circle2d = Handle(Geom2d_Circle)::DownCast(baseCurve);
					auto center = circle2d->Location();
					double radius = circle2d->Radius();
					CDblPoint start;
					CDblPoint end;
					double bugle;
					CalSEHFromArc({ center.X(),center.Y() }, radius, startAng, sweepAng, start, end, bugle);
					cloneEnt->d_ptr->m_nodes[from.sectionNo].m_bulge = bugle;
					cloneEnt->d_ptr->m_nodes.pop_back();
					PlineNode node(osg::Vec3d(startPt.X(), startPt.Y(), 0), bugle);
					cloneEnt->d_ptr->m_nodes.push_back(node);
				}
			}
			
			PlineNode node(osg::Vec3d(pt.X(), pt.Y(), 0), 0.0);
			cloneEnt->d_ptr->m_nodes.push_back(node);
		}
		result.push_back(cloneEnt);
	}
	if (to < end)
	{
		auto cloneEnt = std::static_pointer_cast<Pline2D>(clone());
		cloneEnt->d_ptr->m_nodes.erase(cloneEnt->d_ptr->m_nodes.begin(), cloneEnt->d_ptr->m_nodes.begin() + to.sectionNo + 1);
		auto fp = curve2ds[to.sectionNo]->FirstParameter();
		auto lp = curve2ds[to.sectionNo]->LastParameter();
		if (to.param < lp)
		{
			auto pt = curve2ds[to.sectionNo]->Value(to.param);
			double bugle = 0.0;
			if (d_ptr->m_nodes[to.sectionNo].m_bulge != 0)
			{
				auto curve2dTo = curve2ds[to.sectionNo];
				gp_Pnt2d startPt;
				curve2dTo->D0(curve2dTo->FirstParameter(), startPt);
				gp_Pnt2d endPt;
				curve2dTo->D0(curve2dTo->LastParameter(), endPt);
				double startAng = to.param / M_PI * 180;
				double sweepAng = (lp - to.param) / M_PI * 180;
				while (sweepAng > 360)
					sweepAng -= 360;
				double crossVec = (pt.X() - startPt.X())*(endPt.Y() - startPt.Y()) - (pt.Y() - startPt.Y())*(endPt.X() - startPt.X());
				if (crossVec < 0)
				{
					sweepAng = -sweepAng;
				}
				auto trimmedCurve = Handle(Geom2d_TrimmedCurve)::DownCast(curve2ds[to.sectionNo].get());
				auto baseCurve = trimmedCurve->BasisCurve();
				if (baseCurve->IsKind(STANDARD_TYPE(Geom2d_Circle)))
				{
					auto circle2d = Handle(Geom2d_Circle)::DownCast(baseCurve);
					auto center = circle2d->Location();
					double radius = circle2d->Radius();
					CDblPoint start;
					CDblPoint end;
					CalSEHFromArc({ center.X(),center.Y() }, radius, startAng, sweepAng, start, end, bugle);
					cloneEnt->d_ptr->m_nodes[0].m_point = osg::Vec3d({ endPt.X(),endPt.Y(),0.0 });
					PlineNode node(osg::Vec3d(startPt.X(), startPt.Y(), 0), bugle);
				}
			}
			
			PlineNode node(osg::Vec3d(pt.X(), pt.Y(), 0),bugle);
			cloneEnt->d_ptr->m_nodes.insert(cloneEnt->d_ptr->m_nodes.begin(), node);
		}
		result.push_back(cloneEnt);
	}
	return result;
}

bool Pline2D::offset(double offset, const osg::Vec3d & offsetSide)
{
	auto curves = getCurve2d();
	Geom2dAPI_ProjectPointOnCurve proj;
	gp_Pnt2d offsetPnt(offsetSide.x(), offsetSide.y());
	double minDis;
	auto curve = curves[0];
	gp_Pnt2d projPt;
	gp_Vec2d projOffsetVec;
	int index = 0;
	//得到最近的投影点
	for (int i = 0; i< int(curves.size()); i++)
	{
		proj.Init(offsetPnt, curves[i]);
		gp_Pnt2d nearPt;
		if (proj.NbPoints() > 0)
		{
			nearPt = proj.NearestPoint();
		}
		gp_Vec2d vec(nearPt, offsetPnt);
		if (i == 0)
		{
			minDis = vec.Magnitude();
			projPt = nearPt;
			projOffsetVec = vec;
		}
		if (minDis > vec.Magnitude())
		{
			minDis = vec.Magnitude();
			projOffsetVec = vec;
			projPt = nearPt;
			curve = curves[i];
			index = i;
		}
	}
	SectionParam param;
	bool success = getSectionParam(curves, projPt, param);
	if (!success)
		return false;
	gp_Pnt2d pt;
	gp_Vec2d projPtVec;
	curve->D1(param.param, pt, projPtVec);
	auto V = projPtVec ^projOffsetVec;
	
	//得到所有的实体
	int nodeCount = getNodeCount();
	std::vector<std::shared_ptr<Ent2DCurve>> ents;
	for (int i = 0; i < nodeCount - 1; i++)
	{
		if (d_ptr->m_nodes[i].m_bulge == 0)
		{
			std::shared_ptr<LineStrip2D> line = std::make_shared<LineStrip2D>(getParent());
			std::vector<osg::Vec3d> points;
			points.push_back(d_ptr->m_nodes[i].m_point);
			points.push_back(d_ptr->m_nodes[i + 1].m_point);
			line->setPoints(points);
			ents.push_back(line);
		}
		else
		{
			CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point), osgPointToCDblPoint(d_ptr->m_nodes[i + 1].m_point), d_ptr->m_nodes[i].m_bulge);
			std::shared_ptr<Arc2D> arc2D = std::make_shared<Arc2D>(getParent());
			arc2D->set(osg::Vec3d(arc.center.x, arc.center.y, 0), arc.radius, arc.StartAngle(), arc.EndAngle());
			ents.push_back(arc2D);
		}
	}
	//修改m_nodes.m_point
	for (int j = 0; j < int(ents.size()); j++)
	{
		auto ent1 = ents[j];
		bool success1;
		if (d_ptr->m_nodes[j].m_bulge == 0)
		{
			if (V < 0)
				success1 = ent1->offset(-offset);
			else
				success1 = ent1->offset(offset);
		}		
		else
		{
			auto arc = std::dynamic_pointer_cast<Arc2D>(ents[j]);
			double arcOffset = offset;
			if (V < 0)
				arcOffset = -offset;
			auto startpt = getPointOfAngle(arc->startAngle(), arc->center(), arc->radius());
			gp_Vec2d vec(toPnt2d(d_ptr->m_nodes[j].m_point), toPnt2d(startpt));
			//判断圆弧是否reverse
			if(vec.Magnitude() > 0.001)
				success1 = ent1->offset(arcOffset);
			else
				success1 = ent1->offset(-arcOffset);
		}
			
		if (!success1)
			return false;
		auto curves1 = ent1->getCurve2d();
		auto curve1 = curves1[0];
		//修改起点坐标
		if (j == 0)
		{
			gp_Pnt2d startPt2;
			curve1->D0(curve1->FirstParameter(), startPt2);
			gp_Pnt2d endPt2;
			curve1->D0(curve1->LastParameter(), endPt2);
			osg::Vec3d startPt = osg::Vec3d(startPt2.X(), startPt2.Y(), 0);
			osg::Vec3d endPt = osg::Vec3d(endPt2.X(), endPt2.Y(), 0);
			auto startV = startPt - d_ptr->m_nodes[j].m_point;
			auto endV = endPt - d_ptr->m_nodes[j].m_point;
			if (startV.length() < endV.length())
			{
				d_ptr->m_nodes[j].m_point = startPt;
			}
			else
			{
				d_ptr->m_nodes[j].m_point = endPt;
			}
			continue;
		}
		auto ent2 = ents[j - 1];
		auto curves2 = ent2->getCurve2d();
		auto curve2 = curves2[0];
		Geom2dAPI_InterCurveCurve inter;
		inter.Init(basisCurve(curve1), basisCurve(curve2));
		osg::Vec3d interPt;
		//将交点设为节点
		if (inter.NbPoints() > 0)
		{
			auto pt1 = inter.Point(1);
			osg::Vec3d interPt1 = { double(pt1.X()),double(pt1.Y()),0 };
			if (inter.NbPoints() > 1)
			{
				auto pt2 = inter.Point(2);
				osg::Vec3d interPt2 = { double(pt2.X()),double(pt2.Y()),0 };
				auto V1 = interPt1 - d_ptr->m_nodes[j].m_point;
				auto V2 = interPt2 - d_ptr->m_nodes[j].m_point;
				if (V1.length() < V2.length())
					interPt = interPt1;
				else
					interPt = interPt2;
			}
			else
			{
				interPt = interPt1;
			}
			d_ptr->m_nodes[j].m_point = interPt;
		}
		else
		{
			if (d_ptr->m_nodes[j - 1].m_bulge != 0 && d_ptr->m_nodes[j].m_bulge != 0)
			{
				auto arc2 = std::dynamic_pointer_cast<Arc2D>(ent2);
				auto startpt = getPointOfAngle(arc2->startAngle(), arc2->center(), arc2->radius());
				auto endpt = getPointOfAngle(arc2->endAngle(), arc2->center(), arc2->radius());
				auto startV = startpt - d_ptr->m_nodes[j].m_point;
				auto endV = endpt - d_ptr->m_nodes[j].m_point;
				if (startV.length() > endV.length())
					d_ptr->m_nodes[j].m_point = endpt;
				else
					d_ptr->m_nodes[j].m_point = startpt;
			}
			else
			{
				return false;
			}	
		}
		//修改终点坐标
		if (j == int(ents.size() - 1))
		{
			gp_Pnt2d startPt2;
			curve1->D0(curve1->FirstParameter(), startPt2);
			gp_Pnt2d endPt2;
			curve1->D0(curve1->LastParameter(), endPt2);
			osg::Vec3d startPt = osg::Vec3d(startPt2.X(), startPt2.Y(), 0);
			osg::Vec3d endPt = osg::Vec3d(endPt2.X(), endPt2.Y(), 0);
			auto startV = startPt - d_ptr->m_nodes[j + 1].m_point;
			auto endV = endPt - d_ptr->m_nodes[j + 1].m_point;
			if (startV.length() < endV.length())
			{
				d_ptr->m_nodes[j + 1].m_point = startPt;
			}
			else
			{
				d_ptr->m_nodes[j + 1].m_point = endPt;
			}
		}
	}
	//重新计算bulge
	for (int i = 0; i < nodeCount - 1; i++)
	{
		if ((d_ptr->m_nodes[i]).m_bulge != 0.0)
		{
			auto ent = ents[i];
			auto arc = std::dynamic_pointer_cast<Arc2D>(ent);
			d_ptr->m_nodes[i].m_bulge =
				CalHFromSCEW(osgPointToCDblPoint(d_ptr->m_nodes[i].m_point),
					osgPointToCDblPoint(arc->center()),
					osgPointToCDblPoint(d_ptr->m_nodes[i+1].m_point),
					d_ptr->m_nodes[i].m_bulge, arc->radius());
		}
	}
	addFlags(UPDATE_GEOMETRY);
	return true;
}

std::vector<std::shared_ptr<Ent3DCurve>> Pline2D::to3d()
{
	std::vector<std::shared_ptr<Ent3DCurve>> result;
	int nodeCount = getNodeCount();
	if (nodeCount > 2)
	{
		std::vector<std::shared_ptr<Ent>> ents;
		explode(ents);
		for (auto ent : ents)
		{
			auto ent2d = std::static_pointer_cast<Ent2DCurve>(ent);
			auto ent3ds = ent2d->to3d();
			for (auto ent3d : ent3ds)
			{
				result.push_back(ent3d);
			}
		}
	}
	else
	{
		if (d_ptr->m_nodes[0].m_bulge == 0)
		{
			std::shared_ptr<LineStrip2D> line = std::make_shared<LineStrip2D>(getParent());
			std::vector <osg::Vec3d> points;
			points.push_back(d_ptr->m_nodes[0].m_point);
			points.push_back(d_ptr->m_nodes[1].m_point);
			line->setPoints(points);
			result = line->to3d();
		}
		else
		{
			std::shared_ptr<Arc2D> arc2D = std::make_shared<Arc2D>(getParent());
			CArc2 arc(osgPointToCDblPoint(d_ptr->m_nodes[0].m_point), osgPointToCDblPoint(d_ptr->m_nodes[1].m_point), d_ptr->m_nodes[0].m_bulge);
			arc2D->set(osg::Vec3d(arc.center.x, arc.center.y, 0), arc.radius, arc.StartAngle(), arc.EndAngle());
			result = arc2D->to3d();
		}
	}
	return result;
}


